#include<stdio.h>
#include<Arcroll.h>
#include<Algorithm/delaunay.h>
#include<Mesh_IO/Mesh_IO.h>
#include<Mesh/libcell_iterator.h>
#include<Algorithm/marching_cubes.h>
#include<Algorithm/points_cloud_algorithm.h>
#include<tool/libcell_kd_node_iterator.h>
#include<tool/libcell_octree_iterator.h>
#include<poisson_construct.h>

#include<IterativeLinearSolvers>
#include<Sparse>
#include<vector>


#include<libcell_macros.h>
using namespace Eigen;
//
//3/4-x^2 --- (0 0.5)
//x^2/2-3x/2+9/8 --- (0.5 1.5)
//
//
typedef struct Poisson_Foundamental_Info{
	int depth;
	double width,range,fens;
}Poisson_Foundamental_Info;
static inline  void poisson_foundamental_info_init(Poisson_Foundamental_Info* pfi )
{
	pfi->depth=0 ;
	pfi->range=1.1;
	pfi->fens=0;
	pfi->width=0;
}


typedef struct Poisson_Grid_Prop {
	int indexs[3];
	double normal[3],pos[3];
	int num;
}Poisson_Grid_Prop;
VectorXd test_sparse_matrix_equation(Mesh* m,KD_Node* kdtree,double w );

void test()
{
	Mesh mesh;
	Mesh_init(&mesh);
	_ReadOff_(&mesh,"rabbitCut.off",3);
	delaunay_triangulation_from_mesh(&mesh);	

	//mesh_createconvex(&mesh);

	Viewer_World_Manager vwm;
	viewer_world_manager_init(&vwm);
	Viewer_World *vw=vwm.create_world(&vwm,NULL);

	add_default_somethings(vw);




	Viewer_Opengl_Interpreter voi;
	viewer_opengl_interpreter_initn(&voi,&vwm);
	voi.interpreter(&voi);


	viewer_opengl_interpreter_free(&voi);
	viewer_world_manager_free(&vwm);	
	Mesh_free(&mesh);
}



// void points_cloud_compute_normal_using_eigen(Node* n,KD_Node*kdtree,int num)
// {
// 	double *normal=NULL;
//  	for(Node* nit=n;nit!=NULL;nit=(Node*)(nit->Next))
//     {
//     	template_v* v=(template_v*)(nit->value);
//     	Node* n1=kd_tree_find_n_adjacent_verticesn(v,kdtree,num);
//     	double* axis=compute_pca_result_using_eigen(n1);	
//     	normal=(double*)malloc(sizeof(double)*3);

//     	normal[0]=axis[6];normal[1]=axis[7];normal[2]=axis[8];
//     	v->prop=normal;	
//     	free(axis);
// 		free_node(n1);
//     } 
// } 

void test_add_points(Viewer_World* vw)
{
	Node* n=vw->create_something(vw,"Points");
	Viewer_Something*vs=(Viewer_Something*)(n->value);
	Viewer_Points*vp =(Viewer_Points*)(vs->evolution);
	vp->Data_rows=2 ;
   	vp->Data=(float*)malloc(sizeof(float)*3*vp->Data_rows);
   	vp->color=(float*)malloc(sizeof(float)*4*vp->Data_rows);
	vp->pointsize=12.0;	
	vp->color[3]=1.0;vp->color[7]=1.0;
	vp->Data[0]=-0.6875;vp->Data[1]=0.287992;vp->Data[2]=-0.433799 ;
	vp->Data[3]=-0.627760;vp->Data[4]=0.247154;vp->Data[5]=-0.397233 ;


	free_node(n);
}
Viewer_Something* test_show_mesh_points(Viewer_World* vw,Mesh* m)
{
    Node* n=vw->create_something(vw,"Points");
    Viewer_Something*vs=(Viewer_Something*)(n->value);
    Viewer_Points* vp=(Viewer_Points*)(vs->evolution);
    vp->Data_rows=m->num_v(m);
   	vp->Data=(float*)malloc(sizeof(float)*3*vp->Data_rows);
   	int i=0;
   	vp->color=(float*)malloc(sizeof(float)*4*vp->Data_rows);
   	//memset(vp->color,1,sizeof(float)*4*vp->Data_rows);	
   	for(auto vit=m->v_begin(m);vit!=m->v_end(m);vit++)
   	{
   		for(int j=0;j<3;j++)
   		{
   			vp->Data[i*3+j]=quote(vit)->point[j];
   		}	
   		vp->color[i*4+0]=1.0;vp->color[i*4+1]=0.8;vp->color[i*4+2]=0.0;vp->color[i*4+3]=1.0;
   		i++;
   	} 
    vp->pointsize=10.0;
    free_node(n);
    return vs;
}
void test_show_octree_leafs(Viewer_World* vw,OC_Node* octree)
{
	Node* n=vw->create_something(vw,"Points");
	Viewer_Something*vs=(Viewer_Something*)(n->value);
	Viewer_Points*vp=(Viewer_Points*)(vs->evolution);
	vp->Data_rows=oc_node_voxel_size(octree);
	vp->Data=(float*)malloc(sizeof(float)*3*vp->Data_rows);
   	vp->color=(float*)malloc(sizeof(float)*4*vp->Data_rows);
   	int i=0,num=0;
	for(auto oit=octree_begin(octree);oit!=octree_end(octree);oit++)
   	{
   		if(oit.it->value==NULL)
   		{
   			continue;
   		}
   		num+=node_size(oit.it->value);
  		//printf("depth:%d\n",oit.it->depth); 
   		for(int j=0;j<3;j++)
   		{
   			vp->Data[i*3+j]=(oit.it->loc_min[j]+oit.it->loc_max[j])/2.0;
   		}	
   		vp->color[i*4+0]=0.0;vp->color[i*4+1]=0.8;vp->color[i*4+2]=0.0;vp->color[i*4+3]=1.0;
   		i++;
   	}	
   	printf("i:%d num:%d\n",i,num);
    vp->pointsize=10.0;
	free_node(n);
}

void test_show_octree_leafs_and_normal(Viewer_World* vw,OC_Node* octree)
{
	Node* n=vw->create_something(vw,"Points");
	Viewer_Something*vs=(Viewer_Something*)(n->value);
	Viewer_Points*vp=(Viewer_Points*)(vs->evolution);

   	int i=0,num=0;
   	for(auto oit=octree_begin(octree);oit!=octree_end(octree);oit++)
   	{
   		if(oit.it->prop==NULL)
   		{
   			continue;
   		}
   		i++;	
   	}
   	printf("i:%d\n",i);
	vp->Data_rows=i;
	vp->Data=(float*)malloc(sizeof(float)*3*vp->Data_rows);
   	vp->color=(float*)malloc(sizeof(float)*4*vp->Data_rows);
	i=0,num=0;
	for(auto oit=octree_begin(octree);oit!=octree_end(octree);oit++)
   	{
   		if(oit.it->prop==NULL)
   		{
   			continue;
   		}
   		num+=node_size(oit.it->value);
  		//printf("depth:%d\n",oit.it->depth); 
   		for(int j=0;j<3;j++)
   		{
   			vp->Data[i*3+j]=(oit.it->loc_min[j]+oit.it->loc_max[j])/2.0;
   		}	
   		vp->color[i*4+0]=0.0;vp->color[i*4+1]=0.8;vp->color[i*4+2]=0.0;vp->color[i*4+3]=1.0;
   		i++;
   	}	
   	printf("i:%d num:%d\n",i,num);
    vp->pointsize=10.0;
	free_node(n);

	n=vw->create_something(vw,"Edges");
	vs=(Viewer_Something*)(n->value);
	Viewer_Edges* ve=(Viewer_Edges*)(vs->evolution);
	ve->Data_rows=i*2;
	ve->Data_index_rows=i ;
	ve->Data=(float*)malloc(sizeof(float)*3*ve->Data_rows);
	ve->Data_index=(unsigned int*)malloc(sizeof(unsigned int)*2*ve->Data_index_rows);
	ve->color_rows=ve->Data_index_rows;
	ve->set_color(ve,1.0,0.1,0.1,1.0);
	i=0;

	for(auto oit=octree_begin(octree);oit!=octree_end(octree);oit++)
 	{
   		if(oit.it->prop==NULL)
   		{
   			continue;
   		}
 		Poisson_OC_Prop *pop=(Poisson_OC_Prop*)(oit.it->prop);

   		for(int j=0;j<3;j++)
   		{
   			ve->Data[i*6+j]=(oit.it->loc_min[j]+oit.it->loc_max[j])/2.0;
   			ve->Data[i*6+3+j]=ve->Data[i*6+j]+pop->normal[j]*0.1;

   		}
   		// printf("weizhi:%lf %lf %lf\n",ve->Data[i*6+0],ve->Data[i*6+1],ve->Data[i*6+2]);	
   		// printf("normal:%lf %lf %lf\n",pop->normal[0],pop->normal[1],pop->normal[2]);

 		ve->Data_index[i*2]=i*2;ve->Data_index[2*i+1]=2*i+1;
 		i++;
 	}	

	free_node(n);
}
void test_show_grid_normal(Viewer_World*vw,Node* node)
{
	Node*n=vw->create_something(vw,"Edges");
	Viewer_Something* vs=(Viewer_Something*)(n->value);
	Viewer_Edges* ve=(Viewer_Edges*)(vs->evolution);
	ve->Data_rows=node_size(node)*2;
	ve->Data_index_rows=node_size(node);
	ve->color_rows=ve->Data_index_rows;
	ve->Data=(float *)malloc(sizeof(float)*3*ve->Data_rows);	
	ve->Data_index=(unsigned int*)malloc(sizeof(unsigned int)*2*ve->Data_index_rows);
	ve->color=(float*)malloc(sizeof(float)*4*ve->color_rows);
	ve->set_color(ve,1.0,0.0,0.0,1.0);	
	int i=0;
	printf("data rows:%d index rows:%d\n",ve->Data_rows,ve->Data_index_rows);
	for(Node* nit=node;nit!=NULL;nit=(Node*)(nit->Next))
	{
		Poisson_Grid_Prop* pgp=(Poisson_Grid_Prop*)(nit->value);
		for(int j=0;j<3;j++)
		{
			ve->Data[(i*2)*3+j]=pgp->pos[j];
		}
		double*normal=(double*)(pgp->normal);
		for(int j=0;j<3;j++)
		{
			ve->Data[(i*2+1)*3+j]=pgp->pos[j]+normal[j]*0.1;
		}	
		ve->Data_index[i*2]=i*2;
		ve->Data_index[i*2+1]=i*2+1;
		i++;
	}
	free_node(n);
}
void test_show_normal(Viewer_World* vw,Mesh* m)
{
	Node*n2=vw->create_something(vw,"Edges");
	Viewer_Something* vs=(Viewer_Something*)(n2->value);
	Viewer_Edges* ve=(Viewer_Edges*)(vs->evolution);
	ve->Data_rows=m->num_v(m)*2;
	ve->Data_index_rows=m->num_v(m);
	ve->color_rows=ve->Data_index_rows;
	ve->Data=(float *)malloc(sizeof(float)*3*ve->Data_rows);	
	ve->Data_index=(unsigned int*)malloc(sizeof(unsigned int)*2*ve->Data_index_rows);
	ve->color=(float*)malloc(sizeof(float)*4*ve->color_rows);
	ve->set_color(ve,1.0,0.0,0.0,1.0);	
	int i=0;
	printf("data rows:%d index rows:%d\n",ve->Data_rows,ve->Data_index_rows);
	for(auto vit=m->v_begin(m);vit!=m->v_end(m);vit++)
	{
		for(int j=0;j<3;j++)
		{
			ve->Data[(i*2)*3+j]=quote(vit)->point[j];
		}
		double*normal=(double*)(quote(vit)->prop);
		for(int j=0;j<3;j++)
		{
			ve->Data[(i*2+1)*3+j]=quote(vit)->point[j]+normal[j]*0.1;
		}	
		ve->Data_index[i*2]=i*2;
		ve->Data_index[i*2+1]=i*2+1;

		i++;
	}
	free_node(n2);
}


static inline void poisson_grid_prop_init(Poisson_Grid_Prop* pgp)
{
	memset(pgp->indexs,0,sizeof(int)*3);
	memset(pgp->normal,0,sizeof(double)*3);
	pgp->num=0;
}

Poisson_Grid_Prop*** get_poisson_grid_prop_data(Node* node_vertices,int fens,double width,double range,Node**node)
{
	Poisson_Grid_Prop ***data=(Poisson_Grid_Prop***)malloc(sizeof(Poisson_Grid_Prop**)*fens);
	for(int i=0;i<fens;i++)
	{
		data[i]=(Poisson_Grid_Prop**)malloc(sizeof(Poisson_Grid_Prop*)*fens);
		for(int j=0;j<fens;j++)
		{
			data[i][j]=(Poisson_Grid_Prop*)malloc(sizeof(Poisson_Grid_Prop)*fens);
			for(int k=0;k<fens;k++)
			{
				Poisson_Grid_Prop* pgp=&(data[i][j][k]);
				pgp->indexs[0]=i;pgp->indexs[1]=j;pgp->indexs[2]=k;
				pgp->pos[0]=(i+0.5)*width-range;pgp->pos[1]=(j+0.5)*width-range; pgp->pos[2]=(k+0.5)*width-range;
				poisson_grid_prop_init(pgp);
			}
		}
	}
	*node=NULL;
	for(Node* nit=node_vertices;nit!=NULL;nit=(Node*)(nit->Next))
	{
		//printf("once\n");
		template_v* v=(template_v*)(nit->value);
		int i=(v->point[0]+range)/width ,j=(v->point[1]+range)/width,k=(v->point[2]+range)/width;
		Poisson_Grid_Prop* pgp=&(data[i][j][k]);
		if(pgp->num==0)
		{
			*node=node_overlying(*node,pgp);	
		}	
		pgp->num++;
		double* normal=(double*)(v->prop);
		data[i][j][k].normal[0]+= normal[0];data[i][j][k].normal[1]+= normal[1];data[i][j][k].normal[2]+=normal[2];
	}

	for(Node* nit=*node;nit!=NULL;nit=(Node*)(nit->Next))
	{
		Poisson_Grid_Prop* pgp=(Poisson_Grid_Prop*)(nit->value);
		pgp->normal[0]/=((double)pgp->num);	pgp->normal[1]/=((double)pgp->num);	pgp->normal[2]/=((double)pgp->num);	
	}	

	return data;
}

static inline double poisson_FF_deformation(double* center,double w,double *x )
{
	double p[3]={x[0]-center[0],x[1]-center[1],x[2]-center[2]};
	p[0]/=w;p[1]/=w;p[2]/=w;
	return  poisson_FF(p[0])*poisson_FF(p[1])*poisson_FF(p[2]);	
}


double* test_get_fitting_normal(double * p, Poisson_Grid_Prop*** data, Poisson_Foundamental_Info * pfi )
{
	double* re=(double*)malloc(sizeof(double)*3);
	memset(re,0,sizeof(double)*3);
	int in_i=(p[0]+pfi->range)/pfi->width,in_j=(p[1]+pfi->range)/pfi->width,in_k=(p[2]+pfi->range)/pfi->width;
	int i=in_i-1; 
	while(i>=0&&i<pfi->fens&&i<in_i+2)
	{
		int j=in_j-1;
		while(j>=0&&j<pfi->fens&&j<in_j+2)
		{
			int k=in_k-1;
			while(k>=0&&k<pfi->fens&&k<in_k+2)
			{
				Poisson_Grid_Prop* pgp=(Poisson_Grid_Prop*)(&data[i][j][k]);
				double temp=poisson_FF_deformation(pgp->pos,pfi->width,p);	
				re[0]+=pgp->normal[0]*temp;re[1]+=pgp->normal[1]*temp;re[2]+=pgp->normal[2]*temp;
				k++;
			}
			j++;
		}
		i++;
	}
	return re;
}

// qs :: Ord a => [a] -> [a]
// qs []     = []
// qs (x:xs) = qs lt ++ [x] ++ qs gt
//     where
//         lt = [y | y <- xs, y <= x] 
//         gt = [y | y <- xs, y > x]


// f 0 = 0
// f 1 = 1
// f x = (f (x - 1)) + (f (x - 2))

void test_poisson1()
{
	printf("begin test_poisson1\n");
	Mesh mesh;
	Mesh_init(&mesh);
	_ReadOff_(&mesh, "rabbitCut.off",3);
	mesh_adjust_mesh_to_unit_hexahedron(&mesh);	
	template_v ** values=(template_v**)malloc(sizeof(template_v*)*mesh.num_v(&mesh));
	int i=0;
	Node* node_vertices=NULL;
	for(auto vit=mesh.v_begin(&mesh);vit!=mesh.v_end(&mesh);vit++ )
	{
		values[i]=quote(vit);
		node_vertices=node_overlying(node_vertices,quote(vit));
		i++;
	}
	KD_Node*kdtree1= create_kd_tree(values,i,0);
	points_cloud_compute_normal_using_clapack(node_vertices,kdtree1,8);
	double r=compute_visual_avarage_distance(node_vertices);
	adjust_normal_of_node_vertices(node_vertices,r,kdtree1);	

	int depth =9;
	double range=1.1;
	double fens=pow(2,depth-1),width=(2*range)/fens;
	Node* node=NULL;	

	Poisson_Grid_Prop *** data= get_poisson_grid_prop_data(node_vertices,fens,width,range,&node);


		
	Viewer_World_Manager vwm;
	viewer_world_manager_init(&vwm);
	Viewer_World *vw=vwm.create_world(&vwm,NULL);

	add_default_somethings(vw);
	test_show_grid_normal(vw,node);
	 //test_show_normal(vw,&mesh);
	test_show_mesh_points(vw,&mesh);


	//test_show_octree_leafs(vw,octree);
	//test_show_octree_leafs_and_normal(vw,octree);

	//test_add_points(vw);

	Viewer_Opengl_Interpreter voi;
	viewer_opengl_interpreter_initn(&voi,&vwm);
	voi.interpreter(&voi);
	viewer_opengl_interpreter_free(&voi);
	viewer_world_manager_free(&vwm);	
	for(int i=0;i<fens;i++)
	{
		for(int j=0;j<fens;j++)
		{
			SAFE_FREE(data[i][j]);
		}
		SAFE_FREE(data[i]);
	}
	SAFE_FREE(data);
	SAFE_FREE(values);
	free_node(node_vertices);
	free_kdnode(kdtree1);
	Mesh_free(&mesh);
}

void test_poisson()
{
	Mesh mesh;
	Mesh_init(&mesh);
	_ReadOff_(&mesh,"rabbitCut.off",3);
	mesh_adjust_mesh_to_unit_hexahedron(&mesh);	

	template_v ** values=(template_v**)malloc(sizeof(template_v*)*mesh.num_v(&mesh));
	int i=0;
	Node* node_vertices=NULL;
	for(auto vit=mesh.v_begin(&mesh);vit!=mesh.v_end(&mesh);vit++ )
	{
		values[i]=quote(vit);
		node_vertices=node_overlying(node_vertices,quote(vit));
		i++;
	}

	KD_Node*kdtree1= create_kd_tree(values,i,0);

	//******************
	OC_Node* octree=(OC_Node*)malloc(sizeof(OC_Node));
	poisson_oc_node_init_from_node(octree,node_vertices);
	for(i=0;i<9;i++)
	{
		int num_leafs=oc_node_divide_all_leaves(octree);

		printf("num leafs:%d\n",num_leafs);
	}
	//printf("poisson oc width:%lf\n",poisson_oc_node_width(octree));
   	double w= poisson_oc_node_width(octree);	
   	printf("w :%lf\n",w);
   	
	points_cloud_compute_normal_using_clapack(node_vertices,kdtree1,8);

	double r=compute_visual_avarage_distance(node_vertices);
	adjust_normal_of_node_vertices(node_vertices,r,kdtree1);	


	compute_octree_normal(octree,9);

	Mesh* m1= poisson_transfer_octree2vertices(octree,9);
	printf("m1 num v:%d\n",m1->num_v(m1));
	KD_Node*kdtree=create_kd_tree_from_mesh(m1);	
	//double test_x[3]={0.568359,0.35099,0.329046};
	//double test_x[3]={0.580078,0.335562,0.326582};
	 //double test_x[3]={0.552734,0.528414,0.055457};
	double test_x[3]={0.751953,0.340705,0.023415};	
	double * test_y=compute_value_according_basis_function1(test_x,kdtree,w );
	//double * test_y=compute_value_according_basis_function(test_x,octree,9);
	printf("tset_y:%lf %lf %lf\n",test_y[0],test_y[1],test_y[2]);

	normalize(test_y,3);
	printf("norm tset_y:%lf %lf %lf\n",test_y[0],test_y[1],test_y[2]);


	test_sparse_matrix_equation(m1,kdtree,w );



	free_kdnode(kdtree);	
	Mesh_free(m1);
	free(m1);


	Viewer_World_Manager vwm;
	viewer_world_manager_init(&vwm);
	Viewer_World *vw=vwm.create_world(&vwm,NULL);

	add_default_somethings(vw);

	//test_show_normal(vw,&mesh);
	//test_show_mesh_points(vw,&mesh);
	//test_show_octree_leafs(vw,octree);
	test_show_octree_leafs_and_normal(vw,octree);

	//test_add_points(vw);


	Viewer_Opengl_Interpreter voi;
	viewer_opengl_interpreter_initn(&voi,&vwm);
	voi.interpreter(&voi);


	viewer_opengl_interpreter_free(&voi);
	viewer_world_manager_free(&vwm);	


	//free_node_value(node_nomals);
	free_kdnode(kdtree1);
	oc_node_free(octree);

	free_node(node_vertices);
	free(values);
	Mesh_free(&mesh);
}
static inline double alpha_test(double x)
{
	return (x-1-fabs(x-1))/2.0;
}

static inline double my_fun1(double *p)
{
	double vv1=p[0]*p[0]+p[1]*p[1]+p[2]*p[2];
	double vv2=(p[0]-0.5)*(p[0]-0.5)+(p[1]-0.5)*(p[1]-0.5)+(p[2]-0.5)*(p[2]-0.5);

	return alpha_test(vv1)+alpha_test(vv2)+1.0;	
}
static inline double* compute_normal_of_point1(double* p,double step)
{
	double* re=(double*)malloc(sizeof(double)*3);
	double temp_p[3]={0};
	double value=my_fun1(p);
	for(int i=0;i<3;i++)
	{
		memmove(temp_p,p,sizeof(double)*3);
		temp_p[i]+=step;
		double value1=my_fun1(temp_p);
		re[i]=(value1-value)/step;	
	}

	return re;
}


Node* test_marching_cubes()
{
	Node* re=NULL;
	GRIDCELL grid;	
	double step=2.0/100.0;
	for(int i=0;i<100;i++)
	{
		for(int j=0;j<100;j++)
		{
			for(int k=0;k<100;k++)
			{
				grid.p[0][0]=-1.0+i*step; grid.p[0][1]=-1.0+(j+1)*step; grid.p[0][2]=-1.0+k*step;		
				grid.p[1][0]=-1.0+(i+1)*step; grid.p[1][1]=-1.0+(j+1)*step; grid.p[1][2]=-1.0+k*step;
				grid.p[2][0]=-1.0+(i+1)*step; grid.p[2][1]=-1.0+(j)*step; grid.p[2][2]=-1.0+k*step;
				grid.p[3][0]=-1.0+(i)*step; grid.p[3][1]=-1.0+(j)*step; grid.p[3][2]=-1.0+k*step;	
//***************************
				grid.p[4][0]=-1.0+(i)*step; grid.p[4][1]=-1.0+(j+1)*step; grid.p[4][2]=-1.0+(k+1)*step;
				grid.p[5][0]=-1.0+(i+1)*step; grid.p[5][1]=-1.0+(j+1)*step; grid.p[5][2]=-1.0+(k+1)*step;
				grid.p[6][0]=-1.0+(i+1)*step; grid.p[6][1]=-1.0+(j)*step; grid.p[6][2]=-1.0+(k+1)*step;
				grid.p[7][0]=-1.0+(i)*step; grid.p[7][1]=-1.0+(j)*step; grid.p[7][2]=-1.0+(k+1)*step;
				
				for(int l=0;l<8;l++)
				{
					// double vv1=grid.p[l][0]*grid.p[l][0]+grid.p[l][1]*grid.p[l][1]+grid.p[l][2]*grid.p[l][2];
					// double vv2=(grid.p[l][0]-0.5)*(grid.p[l][0]-0.5)+(grid.p[l][1]-0.5)*(grid.p[l][1]-0.5)+(grid.p[l][2]-0.5)*(grid.p[l][2]-0.5);
	
					grid.val[l]=my_fun1(grid.p[l]);
					double *normal=compute_normal_of_point1(grid.p[l],step);	
					memmove(grid.normals[l],normal,sizeof(double)*3);
					free(normal);
					//alpha_test(vv1)+alpha_test(vv2)+1.0;
				}
				TRIANGLE triangles[5];
				int len=polygonise(grid,0.3,triangles);
				for(int l=0;l<len;l++)
				{
					TRIANGLE*triangle=(TRIANGLE*)malloc(sizeof(TRIANGLE));
					memmove(triangle,&triangles[l],sizeof(TRIANGLE));	
					re=node_overlying(re,triangle);	
				}
			}
		}
	}
	return re;

}

void test_show_triangles(Viewer_World* vw, Node* node_tri)
{

 	Node* n=vw->create_something(vw,"Faces");
    Viewer_Something*vs=(Viewer_Something*)(n->value);
    Viewer_Faces* vf=(Viewer_Faces*)(vs->evolution);
     
    vf->Data_rows=node_size(node_tri)*3;
    vf->Data_index_rows=node_size(node_tri);
    printf("node_tr size:%d\n",vf->Data_rows);

    vf->Data=(float*)malloc(sizeof(float)*3*vf->Data_rows);
    //memset(vf->Data,0,sizeof(float)*3*vf->Data_rows);
    vf->Data_index=(unsigned int*)malloc(sizeof(unsigned int)*4*vf->Data_index_rows);
    vf->normal_rows=vf->Data_index_rows;
    vf->normal=(float*)malloc(sizeof(float)*3*vf->normal_rows);
    memset(vf->normal,0,sizeof(float)*3*vf->normal_rows);
    int i=0;
   	for(Node* nit=node_tri;nit!=NULL;nit=(Node*)(nit->Next))
   	{

   		TRIANGLE* triangle=(TRIANGLE*)(nit->value);

   		normalize(triangle->normals[0],3);
   		for(int j=0;j<3;j++)
   		{
   			for(int k=0;k<3;k++)
   			{
   				vf->Data[i*9+j*3+k]=triangle->p[j][k];	

   			}

   			vf->normal[i*3+j]=triangle->normals[0][j];
   		}

   		vf->Data_index[i*4+0]=3; vf->Data_index[i*4+1]=i*3+0;vf->Data_index[i*4+2]=i*3+1;vf->Data_index[i*4+3]=i*3+2;
   		i++;
   	} 
   	// printf("i :%d\n",i);
    //get_data_from_2dim_cell(m,&(vf->Data),&(vf->Data_index));
     
    vf->color_rows=vf->Data_index_rows;

    vf->set_color(vf,0.9,0.6,0.7,1.0);

   

    //vf->compute_normal(vf);
    
    free_node(n); 
}



VectorXd test_sparse_matrix_equation(Mesh* m,KD_Node* kdtree,double w )
{
	template_v** vs=(template_v**)malloc(sizeof(template_v*)*m->num_v(m));
	for(auto vit=m->v_begin(m);vit!=m->v_end(m);vit++)
	{
		vs[quote(vit)->id]=quote(vit);
	}
	Eigen::VectorXd b(m->num_v(m));
	for(int i=0;i<m->num_v(m);i++)
	{
		b.coeffRef(i)=poisson_compute_b_i(vs[i],kdtree,w);	
	}	
	printf("end b\n");

	SparseMatrix<double> A(m->num_v(m),m->num_v(m));
	//std::vector<Triplet<double>> tripletlist;
	A.reserve(VectorXi::Constant(m->num_v(m),140));
	for(int i=0;i<m->num_v(m);i++)
	{	
		if(vs[i]->id!=i)
		{
			printf("cuowu*****************\n");
		}
		Node* n=kd_tree_find_nearest_sphere(vs[i],4*w,kdtree);
		int num=0;
		for(Node* nit=n;nit!=NULL;nit=(Node*)(nit->Next))
		{
			template_v*v=(template_v*)(nit->value);
			double value= poisson_compute_A_i_j(vs[i],v,w);
			if(fabs(value)>1e-9)
			{
				//tripletlist.push_back(Triplet<double>(i,v->id,value));
				// if(i==38597&&v->id==38373)
				// {
				// 	printf("%d %d %lf\n",i,v->id,value);
				// }
				// if(v->id==38597&&i==38373)
				// {
				// 	printf("%d %d %lf\n",i,v->id,value);
				// }
				A.coeffRef(i,v->id)=value;	
			}
			num++;
		}
		// if(num>120)
		// {
		// 	printf("node size %d\n",num);
		// }
		free_node(n);
	}	
	printf("end A\n");
	A.makeCompressed();

	
	printf("end compress\n");	
	ConjugateGradient<SparseMatrix<double>,Eigen::Upper> solver;
	VectorXd x= solver.compute(A).solve(b);	
	VectorXd y=A* x;

	//double test_x[3]={0.751953,0.340705,0.023415};	
	double test_x[3]={vs[556203]->point[0], vs[556203]->point[1],vs[556203]->point[2]};
	double test_y[3]={0};
	Node* n1=compute_support_functions_nodes(test_x,kdtree,w);	
	printf("%lf %lf \n",w,1/w);
	for(Node* nit=n1;nit!=NULL;nit=(Node*)(nit->Next))
	{
	 	template_v* v=(template_v*)(nit->value);
        OC_Node* ocnode=(OC_Node*)(v->prop); 
        //Poisson_OC_Prop* pop=(Poisson_OC_Prop*)(ocnode->prop);
        //为什么由相距1.8的值
        	//为什么相距1.8的也有值
        printf("dis:%lf\n",distance_of_two_points(test_x,v->point,3)/w);
        printf("dis x y z:  %lf %lf %lf\n",(-v->point[0]+test_x[0])/w, (-v->point[1]+test_x[1])/w,(-v->point[2]+test_x[2])/w);

       	// printf("l w h:%lf %lf %lf\n",ocnode->loc_max[0]-ocnode->loc_min[0],ocnode->loc_max[1]-ocnode->loc_min[1],ocnode->loc_max[2]-ocnode->loc_min[2]); 
        // printf("center:%lf %lf %lf\n",(ocnode->loc_max[0]+ocnode->loc_min[0])/2.0, (ocnode->loc_max[1]+ocnode->loc_min[1])/2.0, (ocnode->loc_max[2]+ocnode->loc_min[2])/2.0);
        // printf("testx :%lf %lf %lf\n",test_x[0],test_x[1],test_x[2]);
       
        for(int j=0;j<3;j++)
        {
        // double value=oc_poisson_DFF(test_x,ocnode,j);
        	test_y[j]+=(oc_poisson_DFF(test_x,ocnode,j)*x(v->id));
        	printf(" %d %.9lf x %lf\n",v->id,oc_poisson_DFF(test_x,ocnode,j),x(v->id));
        }
	}	
	//normalize(test_y,3);
	printf("test_y:%lf %lf %lf\n",test_y[0],test_y[1],test_y[2]);

//0.824156 0.557377 0.100491
	free_node(n1);
	// for(int i=0;i<m->num_v(m);i++)
	// {
	// 	// if(fabs(y(i)-b(i))>0.001)
	// 	// {
	// 	// 	printf("cuwouuuuuuuu\n");
	// 	// }
	// 	printf("%lf %lf\n",y(i),b(i));
	// }
 	//Eigen::SimplicialLDLT<Eigen::SparseMatrix<double>>llt;
 	// 	printf("llt begin A\n");
 	//    llt.compute(A);	
	// //Eigen::SimplicialCholesky<SparseMatrix<double>> chol(A);
	 // 	printf("llt end A\n");
	
	// Eigen::VectorXd x=llt.solve(b);
	//chol.solve(b);
	free(vs);
	return x;
}
int  main()
{
	test_poisson1();

	//test_poisson();

	// Node* node_tri=test_marching_cubes();


	// Viewer_World_Manager vwm;
	// viewer_world_manager_init(&vwm);
	// Viewer_World *vw=vwm.create_world(&vwm,NULL);

	// add_default_somethings(vw);

	// test_show_triangles(vw,node_tri);
	

	// Viewer_Opengl_Interpreter voi;
	// viewer_opengl_interpreter_initn(&voi,&vwm);
	// voi.interpreter(&voi);


	// viewer_opengl_interpreter_free(&voi);
	// viewer_world_manager_free(&vwm);	



	// free_node(node_tri);	


	//test();
		
	return 1;
}
